% ***************************************
% Author: Chaoqun Wang
% Date: 2019-10-15
% ***************************************

% 流程初始化
close all; clear; clc;
x_I = 1; y_I = 1;     % 设置初始点
x_G = 700; y_G = 700; % 设置目标点
Thr = 50;             % 设置目标点阈值
Delta = 30;           % 设置扩展步长
r = 100;              % 设置重优化范围

% 建树初始化
T.v(1).x = x_I;     % T是我们要做的树，v是节点，这里先把起始点加入到T里面来
T.v(1).y = y_I;
T.v(1).xPrev = x_I; % 起始节点的父节点仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).indPrev = 1; % 父节点的索引
T.v(1).cost = 0;    % 累积代价

% 开始构建树——作业部分
figure(1);
ImgRgb = imread('newmap.png');
Img = rgb2gray(ImgRgb);
imshow(Img);
xL = size(Img, 1); % 地图x轴长度
yL = size(Img, 2); % 地图y轴长度
hold on
plot(x_I, y_I, 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot(x_G, y_G, 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % 绘制起点和目标点
count = 1;

for iter = 1:3000
    % x_rand = [];
    % Step 1: 在地图中随机采样一个点x_rand
    % 提示: 用（x_rand(1),x_rand(2)）表示环境中采样点的坐标
    x_rand = [randi(xL), randi(yL)];

    % x_near = [];
    % Step 2: 遍历树，从树中找到最近邻近点x_near
    % 提示: x_near已经在树T里
    d_min = Inf;
    i_min = -1;

    for i = 1:count
        d = norm([x_rand(1) - T.v(i).x, x_rand(2) - T.v(i).y]);

        if d < d_min
            d_min = d;
            i_min = i;
        end

    end

    x_near = [T.v(i_min).x, T.v(i_min).y];

    % x_new = [];
    % Step 3: 扩展得到x_new节点
    % 提示: 注意使用扩展步长Delta
    x_dir = x_rand - x_near;
    x_dir = x_dir / norm(x_dir);
    x_new = x_near + Delta * x_dir;

    % 检查节点是否是collision-free
    if ~collisionChecking(x_near, x_new, Img)
        continue;
    end

    count = count + 1;

    % Step 4: 将x_new插入树T
    % 提示: 新节点x_new的父节点是x_near
    T.v(count).x = x_new(1);
    T.v(count).y = x_new(2);
    T.v(count).xPrev = x_near(1);
    T.v(count).yPrev = x_near(2);
    T.v(count).indPrev = i_min;
    T.v(count).cost = T.v(i_min).cost + Delta;

    % Step 5: 重优化节点(rewire)
    % 提示: 在优化范围内更新x_new的父节点和以x_new作为父节点尝试更新更优代价
    for i = 1:count - 1
        node_n = T.v(i);
        d = norm([x_new(1) - node_n.x, x_new(2) - node_n.y]);

        if d < r
            cost = node_n.cost + d;

            if T.v(count).cost > cost && collisionChecking([node_n.x, node_n.y], x_new, Img)
                T.v(count).xPrev = node_n.x;
                T.v(count).yPrev = node_n.y;
                T.v(count).indPrev = i;
                T.v(count).cost = cost;
            else
                cost = T.v(count).cost + d;

                if node_n.cost > cost && collisionChecking([node_n.x, node_n.y], x_new, Img)
                    T.v(i).xPrev = x_new(1);
                    T.v(i).yPrev = x_new(2);
                    T.v(i).indPrev = count;
                    T.v(i).cost = cost;
                end

            end

        end

    end

    % 绘图
    imshow(Img);
    hold on;
    plot(x_I, y_I, 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
    plot(x_G, y_G, 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');

    for i = 1:count
        plot([T.v(i).x; T.v(T.v(i).indPrev).x], [T.v(i).y; T.v(T.v(i).indPrev).y], 'black', 'Linewidth', 3);
    end

    circle(x_new, r);
    hold off;

    % Step 6: 检查是否到达目标点附近
    % 提示: 注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环
    if norm(x_new - [x_G, y_G]) < Thr
        hold on;
        break;
    end

    pause(0.1); % 暂停0.1s，使得RRT_star扩展过程容易观察
end

% 路径已经找到，反向查询
if iter < 2000
    path.pos(1).x = x_G; path.pos(1).y = y_G;
    path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
    pathIndex = T.v(end).indPrev; % 终点加入路径
    j = 3;

    while 1
        path.pos(j).x = T.v(pathIndex).x;
        path.pos(j).y = T.v(pathIndex).y;
        pathIndex = T.v(pathIndex).indPrev;

        if pathIndex == 1
            break
        end

        j = j + 1;
    end % 沿终点回溯到起点

    path.pos(end + 1).x = x_I; path.pos(end).y = y_I; % 起点加入路径

    for j = 2:length(path.pos)
        plot([path.pos(j).x; path.pos(j - 1).x], [path.pos(j).y; path.pos(j - 1).y], 'b', 'Linewidth', 3);
    end

else
    disp('Error, no path found!');
end

function circle(center, radius)
    pos = [center(1) - radius, center(2) - radius, 2 * radius, 2 * radius];
    rectangle('Position', pos, 'Curvature', [1, 1], 'LineStyle', '--', 'EdgeColor', 'r');
end
